#!/usr/bin/env python3

import rospy
import paho.mqtt.client as mqtt
from std_msgs.msg import Float32
from sensor_msgs.msg import NavSatFix
import struct

class MqttBridge(object):
    yaw_offset = 0
    def __init__(self):
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_disconnect = self.on_disconnect
        self.mqtt_client.connect("test.mosquitto.org", 1883, 60)
        self.mqtt_client.loop_start()
        # 初始化yaw的值
        self.yaw = 0

        # 订阅ROS中的GPS话题
        rospy.Subscriber("/ublox_gps/mapfix", NavSatFix, self.gps_callback) 
        rospy.Subscriber("/robot_yaw", Float32, self.yaw_callback) 

    def on_connect(self, client, userdata, flags, rc):
        rospy.logwarn("[ros_to_mqtt]Connected to MQTT broker with result code %s", str(rc))
        client.subscribe("wpr_gps")

    def on_disconnect(self, client, userdata, rc):
        rospy.logwarn("[ros_to_mqtt] Disconnected from MQTT broker with result code %s", str(rc))

    # 从ROS的GPS话题中获取坐标值
    def gps_callback(self, msg):
        self.latitude = msg.latitude
        self.longitude = msg.longitude

    # 从ROS的robot_yaw话题中获取机器人朝向角
    def yaw_callback(self, msg):
        self.yaw = msg.data

    def run(self):
        self.latitude = 0#39.74025 + 0.0125
        self.longitude = 0#116.12685 - 0.0128
        self.yaw = 0
        while not rospy.is_shutdown():
            # 向MQTT网络发布数据
            data = ",".join([str(self.latitude),str(self.longitude),str(self.yaw)])
            self.mqtt_client.publish("wpr_robot_gps", data)
            rospy.sleep(0.1)

if __name__ == '__main__':
    rospy.init_node('ros_to_mqtt')
    mqtt_bridge = MqttBridge()
    mqtt_bridge.run()
